/*
 */
#include <Metro.h> //Include Metro library
#include <Servo.h>

#define CYCLE_PUSH_NUM  4

#define SERVO_NEUTRAL_DEG  90 // 待機時サーボ位置
#define SERVO_PUSH_DEG  75 // PUSH時サーボ-位置
#define SERVO1_PIN  9
#define SERVO2_PIN  10

#define INTERVAL_SERVO_PUSH 200
#define INTERVAL_1PUSH		500	
#define INTERVAL_WAITING  INTERVAL_1PUSH * 4 + 1 /* 処理落ちを考慮してほんの少しだけ遅らせる */

/*
 * |--       |--       |--       |--         <- サーボを動かしている間の時間(SERVO_PUSH)
 * |---------|---------|---------|---------  <- 1サイクルのPUSH4セット+その間(1PUSH)
 *  サイクル間の待ち時間(WAITING) ->       ----------------------------------------
 */

Servo myservo1, myservo2;
Metro m_pushing = Metro(INTERVAL_SERVO_PUSH);
Metro m_cycle_interval = Metro(INTERVAL_WAITING);
Metro m_push_interval = Metro(INTERVAL_1PUSH);

//Create a variable to hold theled's current state
enum{
	ST_INI,
	ST_ACT_PSH,
	ST_ACT_REL,
	ST_WAIT
};
static int state;
static int push_num;

static const int push_pattern[CYCLE_PUSH_NUM] = {0, 0, 0, 1};
#define SERVO_NTL 0
#define SERVO_PSH 1
static const int servo_degs[2][2] = {{65, 58}, {88, 82}};

// 
static void change_state(int next_state);
static void servo_action(int num);

void setup()
{
	myservo1.attach(SERVO1_PIN);
	myservo2.attach(SERVO2_PIN);  

	myservo1.write(SERVO_NEUTRAL_DEG);
	myservo2.write(SERVO_NEUTRAL_DEG);

	Serial.begin(115200);

	state = ST_INI;
}

void loop()
{
	/* INIステーと以外でキャラクタ受信したら初期状態に戻る */
	if(state != ST_INI && Serial.available()){
		Serial.read();
		change_state(ST_INI);
	}

	switch(state){
	case ST_INI:
		if(Serial.available()){
			Serial.read();
			push_num = 0;
			change_state(ST_ACT_PSH);
		}
		break;

	case ST_ACT_PSH:
		if(m_pushing.check()){
			change_state(ST_ACT_REL);
		}
		break;

	case ST_ACT_REL:
		if(m_push_interval.check()){
			if(push_num == CYCLE_PUSH_NUM){
				change_state(ST_WAIT);
			}
			else{
				change_state(ST_ACT_PSH);
			}
		}
		break;

	case ST_WAIT:
		if(m_cycle_interval.check()){
			push_num = 0;
			change_state(ST_ACT_PSH);
		}
		break;
	}
}

static void change_state(int next_state)
{
	//Serial.print("st: ");	Serial.print(state);	Serial.print(" -> ");	Serial.println(next_state);

	state = next_state;
	// ステート遷移時の処理を書く
	switch(next_state){
	case ST_INI:
		myservo1.write(servo_degs[0][0]);
		myservo2.write(servo_degs[1][0]);
		break;
	case ST_ACT_PSH:
		//Serial.print("num: "); Serial.println(push_num);
		servo_action(push_num, SERVO_PSH);
		m_pushing.reset();
		m_push_interval.reset();
		break;
	case ST_ACT_REL:
		servo_action(push_num, SERVO_NTL);
		push_num++;
		break;
	case ST_WAIT:
		m_cycle_interval.reset();
		break;
	}
}

/**
 * 現在のPUSH回数と押す/離すの指示から
 * サーボの角度制御を行う
 * サーボ1個のみ制御
 *
 * @param num 現在何パターン目か(0-CYCLE_PUSH_NUM)
 * @param push_ntl 0:離す, 1:押す
 */
static void servo_action(int num, int push_ntl)
{
	Servo act_servo;
	int servo_num;

	if((num < 0 || num > CYCLE_PUSH_NUM) || (push_ntl < 0 || push_ntl > 1)){
		return;
	}

	servo_num = push_pattern[num];

	switch(servo_num){
	case 0:
		act_servo = myservo1;
		break;
	case 1:
		act_servo = myservo2;
		break;
	}
	//Serial.print("deg: "); Serial.println(servo_degs[servo_num][push_ntl]);
	act_servo.write(servo_degs[servo_num][push_ntl]);
}
